% Mobile translation
clear;
clc;

poscpp = load('Mobile_position.txt'); % C++/asm position

s = size(poscpp);
n = s(1);

dt = load('dt.txt');
pos = [0.278498218867048 0.546881519204984 0.957506835434298]; % Matlab position
P = [0.957166948242946 -0.485375648722841 0.8002804688888];
m = 1.26986816293506;
force = [-0.905791937075619 0.513375856139019 0.63235924622541];

error = zeros(n, 3);

for i=1:n
    P = P + force * dt;
    v = P / m;
    pos = pos + v * dt;
    
    error(i,:) = pos - poscpp(i,:);
end

% final Matlab position
pos

figure(1);
clf;
hold on;
plot(error(:,1)', 'b');
plot(error(:,2)', 'r');
plot(error(:,3)', 'c');
title({'Mobile object coordinates';'(rounding mode: NEAR)'});
xlabel(['iteration (dt = ', num2str(dt, '%.16f'), 's)']);
ylabel('error');
legend('x', 'y', 'z');
